#include "Physics.h"

void CPhysicsPoint::update(Scalar fTimeStep)
{
    
    //Do Verlet integration
	Vector vNewPos = 2.f * m_vPos - m_vOldPos + (m_vForce * m_f1overMass) * fTimeStep * fTimeStep;
	m_vOldPos = m_vPos;
 	m_vPos = vNewPos;
 	m_vForce.zero();
    
    m_vVel = (m_vPos - m_vOldPos) * 0.5f;
    
    //Leapfrog integration scheme
    /*
    //Linear momentum
    Vector vStepDotAccel = fTimeStep * m_vForce * m_f1overMass;
    m_vVel 	  += vStepDotAccel;
 	vPos 	  += fTimeStep * m_vVel;
 	m_vVel    += 0.5 * vStepDotAccel;
 	m_vForce.zero();
 	*/
 	
 	//TODO: Remove or replace this hacked speed limit
 	Scalar fMaxAngVel = 1.2f;
 	if (m_fAngVel > fMaxAngVel)
        m_fAngVel = fMaxAngVel;
    else if (m_fAngVel < -fMaxAngVel)
        m_fAngVel = -fMaxAngVel;
 	
 	//TODO: Think about using velocity verlet for angular momentum
 	
 	//Angular momentum
 	Scalar fStepDotAngAccel = fTimeStep * m_fTorque * m_f1overRotInertia * 0.01f;
    m_fAngVel         += fStepDotAngAccel;
 	m_fAngle          += fTimeStep * m_fAngVel;
 	m_fAngVel         += 0.5 * fStepDotAngAccel;
 	m_fTorque = 0;
    
}
